/*
Copyright 2010 Iain Surgey.

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  

See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

*/
float av_sr, av_sp, av_sy, av_cr, av_cp, av_cy, av_angle, vn_length, vn_ilength;

#__body__

private inline function  VectorLength(v : Int) : Float
{
	#{d| float	length}
	
	length = Memory.getFloat(v)*Memory.getFloat(v); //#{v[i]*v[i]};
	length += Memory.getFloat(v+4)*Memory.getFloat(v+4);
	length += Memory.getFloat(v+8)*Memory.getFloat(v+8);
	
	length = Math.sqrt (length);		// FIXME
	
	return length;
}

private inline function VectorSet(v : Int, v0 : Float, v1 : Float, v2 : Float) {
	
	Memory.setFloat(v, v0);
	Memory.setFloat(v+4, v1);
	Memory.setFloat(v+8, v2);
	
}

private inline function VectorMA (veca : Int, scale : Float, vecb : Int, vecc : Int)
{
	Memory.setFloat(vecc, Memory.getFloat(veca) + scale*Memory.getFloat(vecb));
	Memory.setFloat(vecc+4, Memory.getFloat(veca+4) + scale*Memory.getFloat(vecb+4));
	Memory.setFloat(vecc+8, Memory.getFloat(veca+8) + scale*Memory.getFloat(vecb+8));
}

private inline function VectorPrint(v : Int) {
	
	trace(Memory.getFloat(v)+" | "+Memory.getFloat(v+4)+" | "+Memory.getFloat(v+8));
}


private inline function VectorCopy (inv : Int, out : Int)
{
	Memory.setFloat(out,Memory.getFloat(inv));
	Memory.setFloat(out+4,Memory.getFloat(inv+4));
	Memory.setFloat(out+8,Memory.getFloat(inv+8));
}


private inline function AngleVectors (angles : Int, forward : Int, right : Int, up : Int)
{
	#{av_angle = #[Memory.getFloat(angles+4)]# * (M_PI*2 / 360)};
	#{av_sy = #[Math.sin(av_angle)]#};
	#{av_cy = #[Math.cos(av_angle)]#};
	#{av_angle = #[Memory.getFloat(angles)]# * (M_PI*2 / 360)};
	#{av_sp = #[Math.sin(av_angle)]#};
	#{av_cp = #[Math.cos(av_angle)]#};
	#{av_angle = #[Memory.getFloat(angles+8)]# * (M_PI*2 / 360)};
	#{av_sr = #[Math.sin(av_angle)]#};
	#{av_cr = #[Math.cos(av_angle)]#};

	//if (forward)
	//{
	Memory.setFloat(forward,av_cp*av_cy);
	Memory.setFloat(forward+4,av_cp*av_sy);
	Memory.setFloat(forward+8,-av_sp);
	//}
	//if (right)
	//{
	Memory.setFloat(right,(-1*av_sr*av_sp*av_cy+-1*av_cr*-av_sy));
	Memory.setFloat(right+4,(-1*av_sr*av_sp*av_sy+-1*av_cr*av_cy));
	Memory.setFloat(right+8,-1*av_sr*av_cp);
	//}
	//if (up)
	//{
	Memory.setFloat(up,(av_cr*av_sp*av_cy+-av_sr*-av_sy));
	Memory.setFloat(up+4,(av_cr*av_sp*av_sy+-av_sr*av_cy));
	Memory.setFloat(up+8,av_cr*av_cp);
	//}
}

private inline function DotProduct (v1 : Int, v2 : Int)
{
	return Memory.getFloat(v1)*Memory.getFloat(v2) + Memory.getFloat(v1+4)*Memory.getFloat(v2+4) + Memory.getFloat(v1+8)*Memory.getFloat(v2+8);
}



private inline function VectorNormalize (v : Int)
{
	vn_length = Math.sqrt(Memory.getFloat(v)*Memory.getFloat(v) + Memory.getFloat(v+4)*Memory.getFloat(v+4) 
	+ Memory.getFloat(v+8)*Memory.getFloat(v+8));
	
	//vn_length = VectorLength(v);
	
	vn_ilength = 1/vn_length;
	
	if (vn_length != 0)
	{
		Memory.setFloat(v,Memory.getFloat(v) *vn_ilength);
		Memory.setFloat(v+4,Memory.getFloat(v+4) *vn_ilength);
		Memory.setFloat(v+8,Memory.getFloat(v+8) *vn_ilength);
	}
	
	return vn_length;
}

private inline function VectorSubtract (veca : Int, vecb : Int, out : Int)
{
	Memory.setFloat(out,Memory.getFloat(veca)-Memory.getFloat(vecb));
	Memory.setFloat(out+4,Memory.getFloat(veca+4)-Memory.getFloat(vecb+4));
	Memory.setFloat(out+8,Memory.getFloat(veca+8)-Memory.getFloat(vecb+8));
}

private inline function VectorScale (vin : Int, scale : Float, out : Int)
{
	Memory.setFloat(out,Memory.getFloat(vin)*scale);
	Memory.setFloat(out+4,Memory.getFloat(vin+4)*scale);
	Memory.setFloat(out+8,Memory.getFloat(vin+8)*scale);
}

private inline function VectorInverse (v : Int)
{
	Memory.setFloat(v, -Memory.getFloat(v));
	Memory.setFloat(v+4, -Memory.getFloat(v+4));
	Memory.setFloat(v+8, -Memory.getFloat(v+8));
}

private inline function VectorAdd (veca : Int, vecb : Int, out : Int)
{
	Memory.setFloat(out, Memory.getFloat(veca)+Memory.getFloat(vecb));
	Memory.setFloat(out+4, Memory.getFloat(veca+4)+Memory.getFloat(vecb+4));
	Memory.setFloat(out+8, Memory.getFloat(veca+8)+Memory.getFloat(vecb+8));
}

